/*
* \file coordenada.h
* \brief Clase que se encarga de gestionar las coordenadas de los objetos, asi como sus velocidades.
* \author Jose Alonso Solis-Lemus
*/

#include "coordenada.h"
#include <math.h>

Coordenada::Coordenada()
{
    x=0;
    y=0;
    theta=0;
    vel_x=0;
    vel_y=0;
    vel_theta=0;
}

Coordenada::Coordenada(double _x, double _y){
    // se trata de una pelota
    x=_x;
    y=_y;
    theta=0;
    vel_x=0;
    vel_y=0;
    vel_theta=0;
}

Coordenada::Coordenada(double _x, double _y, double _theta){
    x=_x;
    y=_y;
    theta=_theta;
    vel_x=0;
    vel_y=0;
    vel_theta=0;
}

Coordenada::~Coordenada(){
}

// Operadores
Coordenada Coordenada::operator-(Coordenada _coor){
    Coordenada resp;
    resp.x=x-_coor.x;
    resp.y=y-_coor.y;

    return resp;
}

Coordenada Coordenada::operator=(Coordenada _nueva){
    Coordenada resp;
    resp.x=_nueva.x;
    resp.y=_nueva.y;
    resp.theta=_nueva.theta;
    resp.vel_x=_nueva.vel_x;
    resp.vel_y=_nueva.vel_y;
    resp.vel_theta=_nueva.vel_theta;

    return resp;
}

// funciones
double Coordenada::norm(){
    double val;
    val = pow(x,2) + pow(y,2);
    return sqrt(val);
}

double Coordenada::distance(Coordenada _coor){
    Coordenada c3;
    c3.x = x -_coor.x;
    c3.y= y-_coor.y;
    return c3.norm();
}

void Coordenada::guessVelocity(Coordenada _antes, double t){
    if(t!=0){
        vel_x=(x-_antes.x) / t;
        vel_y=(y-_antes.y) / t;
        vel_theta=((theta-_antes.theta)*M_2_PI) / t;
    }else{
        vel_x=0;
        vel_y=0;
        vel_theta=0;
    }
}
